/**
 * @file mpu6050.c
 * @author MADAOOTAKU
 * @brief STM32 HAL库下MPU6050驱动代码
 * @version 0.1
 * @date 2025-08-26
 * 
 * @copyright Copyright (c) 2025
 * 
 */

/*说明事项*/
/*1. 本库移植至御龙的MSPM0G3507的6050库*/
/*2.带static关键字的都是内部运行的函数，除了开头两个和标注的之外，都不要动*/
/*3. 由于6050的性能不太好，加上我这个是普通欧拉角解算，没有引入四元数，所以roll和pitch的效果可能会很差（反正也没有人拿6050做无人机吧）*/
/*4.暂且通篇用I2C2通信，可改*/

#include "stdlib.h"
#include "mpu6050.h"
#include "i2c.h"
#include <string.h>


/*----------------------I2C底层读写部分（开始）-------------------------*/

/**
 * @brief 此部分代码是最底层的读写函数，如果要移植请优先修改此代码
 * @note 推荐要移植的各位，先我的库丢给AI分析函数调用关系，列个流程图，再按照流程图移植
 * 
 */


/**
 * @brief (内部函数)向MPU6050寄存器写入8位数据数组
 * @param DevAddr 设备地址
 * @param reg_addr 寄存器地址
 * @param reg_data uint8_t类型数据指针
 * @param count 读取数据长度
 */

#define I2C_TX_BUFFER_SIZE 32

static void I2C_WriteReg(uint8_t DevAddr, uint8_t reg_addr, uint8_t *reg_data, uint8_t count)
{
    uint8_t tx_buf[I2C_TX_BUFFER_SIZE];

    // 检查缓冲区是否足够大
    if (count + 1 > I2C_TX_BUFFER_SIZE)
    {
        // 数据长度超出缓冲区，可以在此处理错误
        return;
    }
    tx_buf[0] = reg_addr;
    memcpy(&tx_buf[1], reg_data, count);
    // 调用HAL库函数，注意设备地址必须左移1位
    HAL_I2C_Master_Transmit(&hi2c2, DevAddr << 1, tx_buf, count + 1, HAL_MAX_DELAY);
}

/**
 * @brief (内部函数)从MPU6050寄存器读取8位数据数组
 * @param DevAddr 设备地址
 * @param reg_addr 寄存器地址
 * @param reg_data uint8_t类型数据指针
 * @param count 读取数据长度
 */

static void I2C_ReadReg(uint8_t DevAddr, uint8_t reg_addr, uint8_t *reg_data, uint8_t count)
{
    // 1. 发送要读取的寄存器地址，注意设备地址左移
    HAL_I2C_Master_Transmit(&hi2c2, DevAddr << 1, &reg_addr, 1, HAL_MAX_DELAY);

    // 2. 接收从该寄存器发回的数据，注意设备地址左移
    HAL_I2C_Master_Receive(&hi2c2, DevAddr << 1, reg_data, count, HAL_MAX_DELAY);
}

/*----------------------I2C底层读写部分（结束）-------------------------*/




/*----------------------MPU6050初始化和通信读取数据部分(开始)-------------------------*/

/**
 * @brief 定义单次读写函数
 * 
 */


#define SMPLRT_DIV 0x19
#define MPU_CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define PWR_MGMT_1 0x6B
#define WHO_AM_I 0x75
#define USER_CTRL 0x6A
#define INT_PIN_CFG 0x37

#define imu_adress 0x68 // MPU6050器件地址

/**
 * @brief (内部函数)单次向MPU6050寄存器写入单个8位数据
 * @param reg_addr 寄存器地址
 * @param reg_data 8位数据
 */
static void Single_WriteI2C(uint8_t reg_addr, uint8_t reg_data)
{
    I2C_WriteReg(imu_adress, reg_addr, &reg_data, 1);
}



/**
 * @brief (内部函数)单次从MPU6050寄存器读取单个8位数据
 * @param reg_addr 寄存器地址
 * @return 读取到的8位数据
 */
static uint8_t Single_ReadI2C(uint8_t reg_addr)
{
    uint8_t reg_data = 0;
    I2C_ReadReg(imu_adress, reg_addr, &reg_data, 1);
    return reg_data;
}

/**
 * @brief 初始化MPU6050
 *
 */

// read_imu[] 是一个调试/校验用的缓冲区，记录初始化后各寄存器的实际值
uint8_t read_imu[5];
/**
 * @brief 最核心的API，在主函数中调用来初始化MPU6050，不插设备别调用，否则会阻塞死
 * @note 外部函数
 */
void mpu6050_init(void)
{
    Single_WriteI2C(PWR_MGMT_1, 0x00);   // 关闭所有中断,解除休眠
    Single_WriteI2C(SMPLRT_DIV, 0x09);   // sample rate.  Fsample= 1Khz/(<this value>+1) =1000Hz
    Single_WriteI2C(MPU_CONFIG, 0x06);   //
    Single_WriteI2C(GYRO_CONFIG, 0x18);  //
    Single_WriteI2C(ACCEL_CONFIG, 0x18); //

    // 读取一些寄存器的值进行调试/校验
    read_imu[0] = Single_ReadI2C(PWR_MGMT_1);
    read_imu[1] = Single_ReadI2C(SMPLRT_DIV);
    read_imu[2] = Single_ReadI2C(MPU_CONFIG);
    read_imu[3] = Single_ReadI2C(GYRO_CONFIG);
    read_imu[4] = Single_ReadI2C(ACCEL_CONFIG);
}

/**
 * @brief 读取MPU6050的加速度、角速度和温度数据
 * @param gyro int16_t类型数组指针, 用于存储读取到的角速度数据
 * @param accel int16_t类型数组指针, 用于存储读取到的加速度数据
 * @param temperature float类型指针, 用于存储读取到的温度数据
 */
static void mpu6050_read(int16_t *gyro, int16_t *accel, float *temperature)
{
    uint8_t buf[14];
    int16_t temp;
    I2C_ReadReg(imu_adress, ACCEL_XOUT_H, buf, 14);
    accel[0] = (int16_t)((buf[0] << 8) | buf[1]);
    accel[1] = (int16_t)((buf[2] << 8) | buf[3]);
    accel[2] = (int16_t)((buf[4] << 8) | buf[5]);
    temp = (int16_t)((buf[6] << 8) | buf[7]);
    gyro[0] = (int16_t)((buf[8] << 8) | buf[9]);
    gyro[1] = (int16_t)((buf[10] << 8) | buf[11]);
    gyro[2] = (int16_t)((buf[12] << 8) | buf[13]);
    *temperature = 36.53f + (float)(temp / 340.0f);
}

/*----------------------MPU6050初始化和通信读取数据部分(结束)-------------------------*/



/*--------------------------欧拉角解算部分(开始)--------------------*/


/*---------------6050结构体定义---------------------*/
MPU6050_DEF mpu6050;

/*---------------陀螺仪参数与滤波函数---------------------*/

/*根据需求可改这两个,剩下的宏定义尽量别改*/
#define Offset_Times 500   // 上电校准次数
#define Sampling_Time 0.01 // 采样读取时间10ms
/*关于这个Sampling_Time要注意一下,主函数while()循环里注释掉的那段代码可能要化0.01秒的几倍时间,所以实际采样其实不是
0.01秒的,这也是为什么如果用OLED屏幕来显示数值会相差十倍,如果要验证,可以在keil里debug然后监视变量,或者用占用时间更少的屏幕,或者串口什么的*/
/*---------------------------------------------------------*/


#define GYRO_GATHER 700 // 原来是100
#define RtA 57.324841f
#define AtR 0.0174533f
#define Acc_G 0.0011963f
#define Gyro_G 0.03051756f
#define Gyro_Gr 0.0005426f



#define IIR_ORDER 4 // 使用IIR滤波器的阶数

double b_IIR[IIR_ORDER + 1] = {0.0008f, 0.0032f, 0.0048f, 0.0032f, 0.0008f};   // 系数b
double a_IIR[IIR_ORDER + 1] = {1.0000f, -3.0176f, 3.5072f, -1.8476f, 0.3708f}; // 系数a
double InPut_IIR[3][IIR_ORDER + 1] = {0};
double OutPut_IIR[3][IIR_ORDER + 1] = {0};



/*--------------------滤波方法----------------------------*/

/**
 * @brief 一阶低通滤波
 * @param oldData 上次滤波后的数据
 * @param newData 本次采样到的数据
 * @param lpf_factor 低通滤波系数, 范围0~1, 越大越接近newData, 越小越接近oldData
 * @return 滤波后的数据
 */
static float LPF_1st(float oldData, float newData, float lpf_factor)
{
    return oldData * (1 - lpf_factor) + newData * lpf_factor; // 上个数据*一定的比例+现在的数据*一定的比例
}

/**
 * @brief 一维卡尔曼滤波
 * @param EKF 卡尔曼滤波器结构体指针
 * @param input 当前输入数据
 */
static void kalmanfiter(struct KalmanFilter *EKF, float input)
{
    EKF->NewP = EKF->LastP + EKF->Q;
    EKF->Kg = EKF->NewP / (EKF->NewP + EKF->R);
    EKF->Out = EKF->Out + EKF->Kg * (input - EKF->Out);
    EKF->LastP = (1 - EKF->Kg) * EKF->NewP;
}




/*---------------MPU6050数据处理---------------------*/


/**
 * @brief 主要是去零漂和滤波
 * 
 * 
 * 
 */
static void MPU6050_ReadDatas_Proc(void)
{
    static uint16_t time = 0; // 初始化校准次数
    mpu6050_read(mpu6050.Gyro_Original, mpu6050.Accel_Original, &mpu6050.temperature);
    if (time < Offset_Times) // 计算初始校准值
    {
        time++;
        mpu6050.Accel_Offset[0] += (float)mpu6050.Accel_Original[0] / Offset_Times; // 读取数据计算偏差
        mpu6050.Accel_Offset[1] += (float)mpu6050.Accel_Original[1] / Offset_Times; // 读取数据计算偏差
        mpu6050.Accel_Offset[2] += (float)mpu6050.Accel_Original[2] / Offset_Times; // 读取数据计算偏差
        mpu6050.Gyro_Offset[0] += (float)mpu6050.Gyro_Original[0] / Offset_Times;   // 读取数据计算偏差
        mpu6050.Gyro_Offset[1] += (float)mpu6050.Gyro_Original[1] / Offset_Times;   // 读取数据计算偏差
        mpu6050.Gyro_Offset[2] += (float)mpu6050.Gyro_Original[2] / Offset_Times;   // 读取数据计算偏差
    }
    else
    {                                                          // 加速度值赋值（减去零漂）
        mpu6050.Accel_Calulate[0] = mpu6050.Accel_Original[0]; // - mpu6050.Accel_Offset[0];//角加速度不用
        mpu6050.Accel_Calulate[1] = mpu6050.Accel_Original[1]; // - mpu6050.Accel_Offset[1];
        mpu6050.Accel_Calulate[2] = mpu6050.Accel_Original[2]; // - mpu6050.Accel_Offset[2];
        // 陀螺仪值赋值（减去零漂）
        mpu6050.Gyro_Calulate[0] = mpu6050.Gyro_Original[0] - mpu6050.Gyro_Offset[0];
        mpu6050.Gyro_Calulate[1] = mpu6050.Gyro_Original[1] - mpu6050.Gyro_Offset[1];
        mpu6050.Gyro_Calulate[2] = mpu6050.Gyro_Original[2] - mpu6050.Gyro_Offset[2];

        /***********角加速度滤波***********/

        //角加速度用卡尔曼滤波方法：
        static struct KalmanFilter EKF[3] = {{0.02, 0, 0, 0, 0.001, 0.543}, {0.02, 0, 0, 0, 0.001, 0.543}, {0.02, 0, 0, 0, 0.001, 0.543}};
        kalmanfiter(&EKF[0], (float)mpu6050.Accel_Calulate[0]);
        mpu6050.Accel_Average[0] = (int16_t)EKF[0].Out;
        kalmanfiter(&EKF[1], (float)mpu6050.Accel_Calulate[1]);
        mpu6050.Accel_Average[1] = (int16_t)EKF[1].Out;
        kalmanfiter(&EKF[2], (float)mpu6050.Accel_Calulate[2]);
        mpu6050.Accel_Average[2] = (int16_t)EKF[2].Out;

        /*******************角速度滤波********************/
        static float x, y, z;
        // 陀螺仪值一阶低通滤波（上个数据，现在的数据，互补滤波的系数）也称互补滤波法
        mpu6050.Gyro_Average[0] = LPF_1st(x, mpu6050.Gyro_Calulate[0], 0.386f);
        x = mpu6050.Gyro_Average[0];
        mpu6050.Gyro_Average[1] = LPF_1st(y, mpu6050.Gyro_Calulate[1], 0.386f);
        y = mpu6050.Gyro_Average[1];
        mpu6050.Gyro_Average[2] = LPF_1st(z, mpu6050.Gyro_Calulate[2], 0.386f);
        z = mpu6050.Gyro_Average[2];
    }
}

#define MPU_Aceel_Gyro_Kp 0.95
float pitch2, roll2, Yaw;
float Gyro_Z_Measeure = 0;

/**
 * @brief 最核心的API，在主函数中调用，用来读取欧拉角
 * @note 外部函数
 * 
 * 
 */
void AHRS_Geteuler(void)
{
    MPU6050_ReadDatas_Proc(); // 读取滤波数据

    float ax, ay, az;
    ax = mpu6050.Accel_Average[0];
    ay = mpu6050.Accel_Average[1];
    az = mpu6050.Accel_Average[2];

    // 一、角加速度和角速度解算的 俯仰角 和 横滚角 进行结合
    float pitch1 = RtA * atan(ay / sqrtf(ax * ax + az * az));                      // 俯仰角
    float roll1 = -RtA * atan(ax / sqrtf(ay * ay + az * az));                      // 横滚角
    pitch2 += (mpu6050.Gyro_Average[0]) * 2000 / 32768 * Sampling_Time;            // 俯仰角
    roll2 += (mpu6050.Gyro_Average[1]) * 2000 / 32768 * Sampling_Time;             // 横滚角
    mpu6050.Pitch = pitch1 * MPU_Aceel_Gyro_Kp + pitch2 * (1 - MPU_Aceel_Gyro_Kp); // 俯仰角
    mpu6050.Roll = roll1 * MPU_Aceel_Gyro_Kp + roll2 * (1 - MPU_Aceel_Gyro_Kp);    // 横滚角


    // z轴不需要更改，足够稳定了
    Gyro_Z_Measeure = (mpu6050.Gyro_Calulate[2]) * 2000 / 32768.0;
    Yaw += Gyro_Z_Measeure * Sampling_Time;


    /*下面的0.16667是看情况加的，比如MSPMOG的板子不加这个系数也行，但是C8T6需要加这个*/
    mpu6050.Yaw = Yaw+ Yaw*0.16667;
    
}
